#!/usr/bin/python3

import time
import numpy as np
from scipy.spatial.transform import Rotation


def ToTransformVisForm(v, euler, euler_seq="zyx"):
    """ Vispy form Transform, note 'xyz' is at T[:3, 3]

    Args:
        v ([type]): [description]
        euler ([type]): [description]
        euler_seq (str, optional): [description]. Defaults to "zyx".

    Returns:
        [type]: [description]
    """
    T = np.eye(4)
    T[3, :3] = v
    T[:3, :3] = Rotation.from_euler(euler_seq, euler).as_matrix()
    return T


def ToTransformNormForm(v, euler, euler_seq="zyx"):
    """ General form Transform, note 'xyz' is at T[3, :3]

    Args:
        v ([type]): [description]
        euler ([type]): [description]
        euler_seq (str, optional): [description]. Defaults to "zyx".

    Returns:
        [type]: [description]
    """
    T = np.eye(4)
    T[:3, 3] = v
    T[:3, :3] = Rotation.from_euler(euler_seq, euler).as_matrix()
    return T


def NormToVisForm(T):
    v = T[:3, 3].copy()
    T[:3, 3] = T[3, :3]
    T[3, :3] = v
    return T
